﻿#pragma once

#include <DirectXMath.h>
#include "Vec3.h"
#include "Vec4.h"

namespace Base {

	using namespace DirectX;

	// 四元数
	// https://learn.microsoft.com/en-us/windows/win32/dxmath/ovw-xnamath-reference-functions-quaternion
	struct Quaternion {

		union
		{
			struct
			{
				float x;
				float y;
				float z;
				float w;
			};
			XMFLOAT4 data;
		};

		Quaternion() {}
		Quaternion(float px, float py, float pz, float pw) :
			x(px), y(py), z(pz), w(pw) {}

		void SetIdentity() {
			x = y = z = 0;
			w = 1.0f;
		}

		// 重载 *
		Quaternion operator*(const Quaternion& rhs)const {

			auto l = XMLoadFloat4(&data);
			auto r = XMLoadFloat4(&rhs.data);

			// 注意 XMQuaternionMultiply(Q1,Q2) 返回的是 Q2 * Q1；
			Quaternion result;
			XMStoreFloat4(&result.data, XMQuaternionMultiply(r, l));
			return result;
		}


		// 对 vec 进行旋转
		// 假定 this 是normalized
		// v1 = q * v * q^-1
		// 返回 v1
		Vec3 Rotate(const Vec3& vec) {
			Quaternion inv(-x, -y, -z, w);
			Quaternion mid(vec.x, vec.y, vec.z, 0);
			Quaternion result = *this * mid * inv;
			return Vec3(result.x, result.y, result.z);
		}

		// Vec4 xyz表示轴  w表示角 弧度单位
		Vec4 ToAxisAngle() const {
			float rad = 0;
			XMVECTOR v;
			XMQuaternionToAxisAngle(&v, &rad, XMLoadFloat4(&data));
			Vec3 axis;
			XMStoreFloat3(&axis.data, v);
			axis.Normalize();
			return Vec4(axis.x, axis.y, axis.z, rad);
		}

		static Quaternion Identity() {
			return Quaternion(0, 0, 0, 1);
		}

		static Quaternion RotateX(float rad) {
			return Quaternion(sinf(rad * 0.5f), 0, 0, cosf(rad * 0.5f));
		}
		static Quaternion RotateY(float rad) {
			return Quaternion(0, sinf(rad * 0.5f), 0, cosf(rad * 0.5f));
		}
		static Quaternion RotateZ(float rad) {
			return Quaternion(0, 0, sinf(rad * 0.5f), cosf(rad * 0.5f));
		}

		static Quaternion CreateFromRollPitchYaw(float rollRad, float pitchRad, float yawRad);
	};
}

